clear all;clc;close all;
x=[-6:0.3:6];
y=[-6:0.3:6];
[XX YY]=meshgrid(x,y);
Z=XX.^2+YY.^2;

%% Step1 initlized
X=[-1.9432   -2.8323
    -3.1588    3.9609
 -0.5896    2.3563    
 -3.7746   -1.9976
-2.1089    0.9628];
V= [   0.9416   -1.5665
   -1.2590    1.1927
    1.1148   -2.1605
   -1.1472   -0.3289
    2.6562    0.8763];
[pbest id]=min(fitfun(X));
Bound=[-6 6;-6 6];

Xpbest=X(id,:);       Xgbest=Xpbest;
gbest=pbest;

%% Step update
N=0;MatPbest=[];MatGbest=[];
while(N<50)
    N=N+1;
w=0.8;c1=2;c2=2;
V=w*V+c1*rand(size(X)).*(repmat(Xpbest,size(X,1),1)-X)+c2*rand(size(X)).*(repmat(Xgbest,size(X,1),1)-X);
X=X+V;


%check outbound
for i=1:size(X,1)
    for j=1:size(X,2)
        if(X(i,j)<Bound(j,1)||X(i,j)>Bound(j,2))
            X(i,j)=X(i,j)-V(i,j);
        end
    end      
end

figure(1)
clf

h=contour(XX,YY,Z,60);
% set(h,'FaceLighting','phong','FaceColor','interp',...
%        'AmbientStrength',0.4)
 light('Position',[0 0 1],'Style','infinite');
% view(-75,45)
axis equal
colorbar
hold on

[pbest id]=min(fitfun(X));


Xpbest=X(id,:);      

if(pbest<gbest)
    gbest=pbest;
     Xgbest=Xpbest;
end

plot3(Xpbest(1),Xpbest(2),pbest,'--ks','LineWidth',2,...
                'MarkerEdgeColor','r',...
                'MarkerFaceColor','r',...
                'MarkerSize',10);
     
plot3(Xgbest(1),Xgbest(2),gbest,'--ks','LineWidth',2,...
                'MarkerEdgeColor','b',...
                'MarkerFaceColor','b',...
                'MarkerSize',12);


h=plot3(X(:,1),X(:,2),fitfun(X),'ro','LineWidth',2,...
                'MarkerEdgeColor','g',...
                'MarkerFaceColor','g',...
                'MarkerSize',8);

legend('Contour','p_b_e_s_t','g_b_e_s_t','Particles')
drawnow
MatPbest=[MatPbest pbest];
MatGbest=[MatGbest gbest];
figure(2)
clf
hold on
plot(1:length(MatPbest),MatPbest,'--rs','LineWidth',2,...
                'MarkerEdgeColor','k',...
                'MarkerFaceColor','g',...
                'MarkerSize',8);
plot(1:length(MatGbest),MatGbest,'--bo','LineWidth',2,...
                'MarkerEdgeColor','k',...
                'MarkerFaceColor','g',...
                'MarkerSize',8);
set(gca,'fontsize',14);xlabel('Generation');
legend('P_b_e_s_t','G_b_e_s_t');            drawnow;
            
end
%%
pbest
Xpbest